// 2025/4/7: 首个版本
// 2025/6/12: 协议名常量改为public

package com.appox.samples;

import com.appox.Sample;
import com.appox.GeneralSample;
import com.appox.GeneralElement;
import java.util.Date;
import java.util.Calendar;
import java.util.GregorianCalendar;

/**
 * GNSS和IMU融合数据样本类
 */
public class GnssImuSample extends Sample {

    /**
     * GNSS/IMU位置模式
     */
    public enum GnssImuLocationMode {
        NO_LOCATION(0), // 无位置信息
        NORMAL(1),      // 默认模式
        RTK_FIXED(2),   // RTK固定解
        RTK_FLOAT(3),   // RTK浮动解
        RTD(4),         // RTD
        IMU_ONLY(5),    // 仅惯导
        MODIFIED(6);    // 后期修正
        
        private final int value;
        
        GnssImuLocationMode(int value) {
            this.value = value;
        }
        
        public int getValue() {
            return value;
        }
        
        public static GnssImuLocationMode fromValue(int value) {
            for (GnssImuLocationMode mode : GnssImuLocationMode.values()) {
                if (mode.getValue() == value) {
                    return mode;
                }
            }
            return NO_LOCATION;
        }
    }
    
    /**
     * GNSS时间类型
     */
    public enum GnssTimeType {
        UTC(0), // UTC时间
        GPS(1), // GPS时间
        TAI(2); // TAI时间
        
        private final int value;
        
        GnssTimeType(int value) {
            this.value = value;
        }
        
        public int getValue() {
            return value;
        }
        
        public static GnssTimeType fromValue(int value) {
            for (GnssTimeType type : GnssTimeType.values()) {
                if (type.getValue() == value) {
                    return type;
                }
            }
            return UTC;
        }
    }
    
    /**
     * 速度分量结构
     */
    public static class SpeedComponents {
        private double east;  // 东向速度分量
        private double north; // 北向速度分量
        private double up;    // 上向速度分量
        
        public SpeedComponents() {
            this.east = 0.0;
            this.north = 0.0;
            this.up = 0.0;
        }
        
        public SpeedComponents(double east, double north, double up) {
            this.east = east;
            this.north = north;
            this.up = up;
        }
        
        public double getEast() {
            return east;
        }
        
        public void setEast(double east) {
            this.east = east;
        }
        
        public double getNorth() {
            return north;
        }
        
        public void setNorth(double north) {
            this.north = north;
        }
        
        public double getUp() {
            return up;
        }
        
        public void setUp(double up) {
            this.up = up;
        }
    }
    
    // 位置相关属性
    private GnssImuLocationMode locationMode; // 位置模式
    private Integer satelliteCount;          // 卫星数量
    
    private Double longitude;                // [°E] 车辆坐标系原点（车前保中心地面）的经度
    private Double latitude;                 // [°N] 车辆坐标系原点（车前保中心地面）的纬度
    private Double altitude;                 // [m] 车辆坐标系原点（车前保中心地面）的海拔
    
    private Double antennaLongitude;         // [°E] 设备坐标系原点（可为天线位置或后轴中心等）的经度
    private Double antennaLatitude;          // [°N] 设备坐标系原点（可为天线位置或后轴中心等）的纬度
    private Double antennaAltitude;          // [m] 设备坐标系原点（可为天线位置或后轴中心等）的海拔
    
    private Double horizontalError;          // [m] 水平位置误差
    private Double verticalError;            // [m] 垂直位置误差
    
    // 运动相关属性
    private Double speed;                    // [kph] 车速
    
    private Double orientation;              // [deg -180~180] 朝向 CCW为正 北为0
    private Double pitch;                    // [deg] 俯仰角 车头朝下为正
    private Double roll;                     // [deg] 横滚角 右侧朝下为正
    
    private Double yawRate;                  // [deg/s] 横摆角速度 左转为正
    private Double pitchRate;                // [deg/s] 俯仰角速度 车头朝下为正
    private Double rollRate;                 // [deg/s] 横滚角速度 右侧朝下为正
    
    private Double ax;                       // [m/s2] 纵向加速度 朝前为正
    private Double ay;                       // [m/s2] 横向加速度 朝左为正
    private Double az;                       // [m/s2] 天向加速度 朝上为正
    
    private Double curvature;                // [1/m] 行驶曲率 左转为正
    
    private Double slipAngle;                // [deg] 侧偏角 左转时为正
    private Double jerkX;                    // [m/s3] 纵向急动度 朝前为正
    private Double jerkY;                    // [m/s3] 横向急动度 朝左为正
    
    // 时间相关属性
    private Double arrivalTime;              // [s] 数据到达时间戳（相对Session开始时间）
    private GnssTimeType gnssTimeType;       // 卫星时间类型
    private Date gnssTime;                   // 卫星时间
    
    /**
     * 构造函数
     */
    public GnssImuSample() {
        this.locationMode = GnssImuLocationMode.NO_LOCATION;
        this.gnssTimeType = GnssTimeType.UTC;
    }
    
    /**
     * 获取转换为UTC的卫星时间
     */
    public Date getGnssTimeUTC() {
        if (gnssTime != null) {
            Calendar calendar = Calendar.getInstance();
            calendar.setTime(gnssTime);
            
            if (gnssTimeType == GnssTimeType.GPS) {
                // GPS时间比UTC时间早18秒
                calendar.add(Calendar.SECOND, -18);
                return calendar.getTime();
            } else if (gnssTimeType == GnssTimeType.TAI) {
                // TAI时间比UTC时间早37秒
                calendar.add(Calendar.SECOND, -37);
                return calendar.getTime();
            } else {
                return gnssTime;
            }
        }
        return null;
    }
    
    /**
     * 获取东北天向速度
     */
    public SpeedComponents getENUSpeed() {
        if (speed != null && orientation != null && pitch != null) {
            SpeedComponents comps = new SpeedComponents();
            double coef = Math.PI / 180;
            
            comps.up = -speed * Math.sin(pitch * coef);
            double hSpeed = speed * Math.cos(pitch * coef);
            comps.north = hSpeed * Math.cos(orientation * coef);
            comps.east = -hSpeed * Math.sin(orientation * coef);
            
            return comps;
        }
        return null;
    }
    
    // Getter和Setter方法
    public GnssImuLocationMode getLocationMode() {
        return locationMode;
    }
    
    public void setLocationMode(GnssImuLocationMode locationMode) {
        this.locationMode = locationMode;
    }
    
    public Integer getSatelliteCount() {
        return satelliteCount;
    }
    
    public void setSatelliteCount(Integer satelliteCount) {
        this.satelliteCount = satelliteCount;
    }
    
    public Double getLongitude() {
        return longitude;
    }
    
    public void setLongitude(Double longitude) {
        this.longitude = longitude;
    }
    
    public Double getLatitude() {
        return latitude;
    }
    
    public void setLatitude(Double latitude) {
        this.latitude = latitude;
    }
    
    public Double getAltitude() {
        return altitude;
    }
    
    public void setAltitude(Double altitude) {
        this.altitude = altitude;
    }
    
    public Double getAntennaLongitude() {
        return antennaLongitude;
    }
    
    public void setAntennaLongitude(Double antennaLongitude) {
        this.antennaLongitude = antennaLongitude;
    }
    
    public Double getAntennaLatitude() {
        return antennaLatitude;
    }
    
    public void setAntennaLatitude(Double antennaLatitude) {
        this.antennaLatitude = antennaLatitude;
    }
    
    public Double getAntennaAltitude() {
        return antennaAltitude;
    }
    
    public void setAntennaAltitude(Double antennaAltitude) {
        this.antennaAltitude = antennaAltitude;
    }
    
    public Double getHorizontalError() {
        return horizontalError;
    }
    
    public void setHorizontalError(Double horizontalError) {
        this.horizontalError = horizontalError;
    }
    
    public Double getVerticalError() {
        return verticalError;
    }
    
    public void setVerticalError(Double verticalError) {
        this.verticalError = verticalError;
    }
    
    public Double getSpeed() {
        return speed;
    }
    
    public void setSpeed(Double speed) {
        this.speed = speed;
    }
    
    public Double getOrientation() {
        return orientation;
    }
    
    public void setOrientation(Double orientation) {
        this.orientation = orientation;
    }
    
    public Double getPitch() {
        return pitch;
    }
    
    public void setPitch(Double pitch) {
        this.pitch = pitch;
    }
    
    public Double getRoll() {
        return roll;
    }
    
    public void setRoll(Double roll) {
        this.roll = roll;
    }
    
    public Double getYawRate() {
        return yawRate;
    }
    
    public void setYawRate(Double yawRate) {
        this.yawRate = yawRate;
    }
    
    public Double getPitchRate() {
        return pitchRate;
    }
    
    public void setPitchRate(Double pitchRate) {
        this.pitchRate = pitchRate;
    }
    
    public Double getRollRate() {
        return rollRate;
    }
    
    public void setRollRate(Double rollRate) {
        this.rollRate = rollRate;
    }
    
    public Double getAx() {
        return ax;
    }
    
    public void setAx(Double ax) {
        this.ax = ax;
    }
    
    public Double getAy() {
        return ay;
    }
    
    public void setAy(Double ay) {
        this.ay = ay;
    }
    
    public Double getAz() {
        return az;
    }
    
    public void setAz(Double az) {
        this.az = az;
    }
    
    public Double getCurvature() {
        return curvature;
    }
    
    public void setCurvature(Double curvature) {
        this.curvature = curvature;
    }
    
    public Double getSlipAngle() {
        return slipAngle;
    }
    
    public void setSlipAngle(Double slipAngle) {
        this.slipAngle = slipAngle;
    }
    
    public Double getJerkX() {
        return jerkX;
    }
    
    public void setJerkX(Double jerkX) {
        this.jerkX = jerkX;
    }
    
    public Double getJerkY() {
        return jerkY;
    }
    
    public void setJerkY(Double jerkY) {
        this.jerkY = jerkY;
    }
    
    public Double getArrivalTime() {
        return arrivalTime;
    }
    
    public void setArrivalTime(Double arrivalTime) {
        this.arrivalTime = arrivalTime;
    }
    
    public GnssTimeType getGnssTimeType() {
        return gnssTimeType;
    }
    
    public void setGnssTimeType(GnssTimeType gnssTimeType) {
        this.gnssTimeType = gnssTimeType;
    }
    
    public Date getGnssTime() {
        return gnssTime;
    }
    
    public void setGnssTime(Date gnssTime) {
        this.gnssTime = gnssTime;
    }
    
    /**
     * 获取通用样本协议
     */
    @Override
    public String getGeneralSampleProtocol() {
        return TARGET_PROTOCOL;
    }
    
    /**
     * 获取通用样本协议列表
     */
    @Override
    public String[] getGeneralSampleProtocols() {
        return PROTOCOLS;
    }

    /**
     * 将GNSS/IMU数据转换为通用样本
     * @param channel 通道号
     */
    public GeneralSample toGeneralSample(int channel) {
        GeneralSample sample = new GeneralSample();
        sample.setTime(this);
        sample.setProtocol(TARGET_PROTOCOL);
        sample.setChannel(channel);
        
        GeneralElement[] data = new GeneralElement[40];
        
        // 初始化所有元素
        for (int i = 0; i < data.length; i++) {
            data[i] = new GeneralElement();
        }
        
        data[0] = new GeneralElement((double)locationMode.getValue());
        if (longitude != null) data[1] = new GeneralElement(longitude);
        if (latitude != null) data[2] = new GeneralElement(latitude);
        if (altitude != null) data[3] = new GeneralElement(altitude);
        if (speed != null) data[4] = new GeneralElement(speed);
        if (orientation != null) data[5] = new GeneralElement(orientation);
        if (pitch != null) data[6] = new GeneralElement(pitch);
        if (roll != null) data[7] = new GeneralElement(roll);
        if (yawRate != null) data[8] = new GeneralElement(yawRate);
        if (ax != null) data[9] = new GeneralElement(ax);
        if (ay != null) data[10] = new GeneralElement(ay);
        if (az != null) data[11] = new GeneralElement(az);
        if (slipAngle != null) data[12] = new GeneralElement(slipAngle);
        if (jerkX != null) data[13] = new GeneralElement(jerkX);
        if (jerkY != null) data[14] = new GeneralElement(jerkY);
        if (satelliteCount != null) data[15] = new GeneralElement((double)satelliteCount);
        if (antennaLongitude != null) data[16] = new GeneralElement(antennaLongitude);
        if (antennaLatitude != null) data[17] = new GeneralElement(antennaLatitude);
        if (antennaAltitude != null) data[18] = new GeneralElement(antennaAltitude);
        
        data[19] = new GeneralElement((double)gnssTimeType.getValue());
        if (arrivalTime != null) data[20] = new GeneralElement(arrivalTime);
        
        if (gnssTime != null) {
            Calendar calendar = Calendar.getInstance();
            calendar.setTime(gnssTime);
            
            data[21] = new GeneralElement((double)calendar.get(Calendar.YEAR));
            data[22] = new GeneralElement((double)(calendar.get(Calendar.MONTH) + 1)); // Calendar月份从0开始
            data[23] = new GeneralElement((double)calendar.get(Calendar.DAY_OF_MONTH));
            data[24] = new GeneralElement((double)calendar.get(Calendar.HOUR_OF_DAY));
            data[25] = new GeneralElement((double)calendar.get(Calendar.MINUTE));
            data[26] = new GeneralElement((double)calendar.get(Calendar.SECOND));
            data[27] = new GeneralElement((double)calendar.get(Calendar.MILLISECOND));
        }
        
        if (pitchRate != null) data[28] = new GeneralElement(pitchRate);
        if (rollRate != null) data[29] = new GeneralElement(rollRate);
        if (horizontalError != null) data[30] = new GeneralElement(horizontalError);
        if (verticalError != null) data[31] = new GeneralElement(verticalError);
        if (curvature != null) data[32] = new GeneralElement(curvature);
        
        sample.setValues(data);
        sample.setNumberOfSignificants(40);
        
        return sample;
    }
    
    /**
     * 获取数据到达时间
     */
    public static Double getArrivalTime(GeneralSample sample) {
        String protocol = sample.getProtocol();
        GeneralElement[] values = sample.getValues();
        
        switch (protocol) {
            case "gnssimu-sample-v7":
                if (values.length != 40) return null;
                return values[20].isValid() ? values[20].getValue() : null;
                
            case "gnssimu-sample-v6":
                if (values.length != 28) return null;
                return values[20].isValid() ? values[20].getValue() : null;
                
            case "gnssimu-sample-v5":
            case "gnssimu-sample-v4":
                return sample.getOffset();
                
            default:
                return null;
        }
    }
    
    /**
     * 从通用样本转换为GNSS/IMU数据
     */
    @Override
    public boolean fromGeneralSample(GeneralSample sample) {
        setTime(sample);
        
        String protocol = sample.getProtocol();
        
        // 根据协议版本调用相应的处理函数
        if ("gnssimu-sample-v7".equals(protocol)) {
            return fromGnssImuSampleV7(sample);
        } else if ("gnssimu-sample-v6".equals(protocol)) {
            return fromGnssImuSampleV6(sample);
        } else if ("gnssimu-sample-v5".equals(protocol)) {
            return fromGnssImuSampleV5(sample);
        } else if ("gnssimu-sample-v4".equals(protocol)) {
            return fromGnssImuSampleV4(sample);
        }
        
        return false;
    }
    
    /**
     * 从通用样本V7版本转换为GNSS/IMU数据
     */
    private boolean fromGnssImuSampleV7(GeneralSample sample) {
        GeneralElement[] values = sample.getValues();
        if (values.length != 40) return false;
        
        locationMode = values[0].isValid() ? GnssImuLocationMode.fromValue((int)values[0].getValue()) : GnssImuLocationMode.NO_LOCATION;
        longitude = values[1].isValid() ? values[1].getValue() : null;
        latitude = values[2].isValid() ? values[2].getValue() : null;
        altitude = values[3].isValid() ? values[3].getValue() : null;
        speed = values[4].isValid() ? values[4].getValue() : null;
        orientation = values[5].isValid() ? values[5].getValue() : null;
        pitch = values[6].isValid() ? values[6].getValue() : null;
        roll = values[7].isValid() ? values[7].getValue() : null;
        yawRate = values[8].isValid() ? values[8].getValue() : null;
        ax = values[9].isValid() ? values[9].getValue() : null;
        ay = values[10].isValid() ? values[10].getValue() : null;
        az = values[11].isValid() ? values[11].getValue() : null;
        slipAngle = values[12].isValid() ? values[12].getValue() : null;
        jerkX = values[13].isValid() ? values[13].getValue() : null;
        jerkY = values[14].isValid() ? values[14].getValue() : null;
        satelliteCount = values[15].isValid() ? (int)values[15].getValue() : null;
        antennaLongitude = values[16].isValid() ? values[16].getValue() : null;
        antennaLatitude = values[17].isValid() ? values[17].getValue() : null;
        antennaAltitude = values[18].isValid() ? values[18].getValue() : null;
        
        gnssTimeType = values[19].isValid() ? GnssTimeType.fromValue((int)values[19].getValue()) : GnssTimeType.UTC;
        arrivalTime = values[20].isValid() ? values[20].getValue() : null;
        
        if (values[21].isValid() && values[22].isValid() && values[23].isValid() &&
            values[24].isValid() && values[25].isValid() && values[26].isValid() && values[27].isValid()) {
            gnssTime = genDateTime((int)values[21].getValue(),
                                   (int)values[22].getValue(),
                                   (int)values[23].getValue(),
                                   (int)values[24].getValue(),
                                   (int)values[25].getValue(),
                                   (int)values[26].getValue(),
                                   (int)values[27].getValue());
        } else {
            gnssTime = null;
        }
        
        pitchRate = values[28].isValid() ? values[28].getValue() : null;
        rollRate = values[29].isValid() ? values[29].getValue() : null;
        horizontalError = values[30].isValid() ? values[30].getValue() : null;
        verticalError = values[31].isValid() ? values[31].getValue() : null;
        curvature = values[32].isValid() ? values[32].getValue() : null;
        
        return true;
    }
    
    /**
     * 从通用样本V6版本转换为GNSS/IMU数据
     */
    private boolean fromGnssImuSampleV6(GeneralSample sample) {
        GeneralElement[] values = sample.getValues();
        if (values.length != 28) return false;
        
        locationMode = values[0].isValid() ? GnssImuLocationMode.fromValue((int)values[0].getValue()) : GnssImuLocationMode.NO_LOCATION;
        longitude = values[1].isValid() ? values[1].getValue() : null;
        latitude = values[2].isValid() ? values[2].getValue() : null;
        altitude = values[3].isValid() ? values[3].getValue() : null;
        speed = values[4].isValid() ? values[4].getValue() : null;
        orientation = values[5].isValid() ? values[5].getValue() : null;
        pitch = values[6].isValid() ? values[6].getValue() : null;
        roll = values[7].isValid() ? values[7].getValue() : null;
        yawRate = values[8].isValid() ? values[8].getValue() : null;
        ax = values[9].isValid() ? values[9].getValue() : null;
        ay = values[10].isValid() ? values[10].getValue() : null;
        az = values[11].isValid() ? values[11].getValue() : null;
        slipAngle = values[12].isValid() ? values[12].getValue() : null;
        jerkX = values[13].isValid() ? values[13].getValue() : null;
        jerkY = values[14].isValid() ? values[14].getValue() : null;
        satelliteCount = values[15].isValid() ? (int)values[15].getValue() : null;
        antennaLongitude = values[16].isValid() ? values[16].getValue() : null;
        antennaLatitude = values[17].isValid() ? values[17].getValue() : null;
        antennaAltitude = values[18].isValid() ? values[18].getValue() : null;
        
        gnssTimeType = values[19].isValid() ? GnssTimeType.fromValue((int)values[19].getValue()) : GnssTimeType.UTC;
        arrivalTime = values[20].isValid() ? values[20].getValue() : null;
        
        if (values[21].isValid() && values[22].isValid() && values[23].isValid() &&
            values[24].isValid() && values[25].isValid() && values[26].isValid() && values[27].isValid()) {
            gnssTime = genDateTime((int)values[21].getValue(),
                                   (int)values[22].getValue(),
                                   (int)values[23].getValue(),
                                   (int)values[24].getValue(),
                                   (int)values[25].getValue(),
                                   (int)values[26].getValue(),
                                   (int)values[27].getValue());
        } else {
            gnssTime = null;
        }
        
        return true;
    }
    
    /**
     * 从通用样本V5版本转换为GNSS/IMU数据
     */
    private boolean fromGnssImuSampleV5(GeneralSample sample) {
        GeneralElement[] values = sample.getValues();
        if (values.length != 19) return false;
        
        locationMode = values[0].isValid() ? GnssImuLocationMode.fromValue((int)values[0].getValue()) : GnssImuLocationMode.NO_LOCATION;
        longitude = values[1].isValid() ? values[1].getValue() : null;
        latitude = values[2].isValid() ? values[2].getValue() : null;
        antennaAltitude = values[3].isValid() ? values[3].getValue() : null;
        speed = values[4].isValid() ? values[4].getValue() : null;
        orientation = values[5].isValid() ? values[5].getValue() : null;
        pitch = values[6].isValid() ? values[6].getValue() : null;
        roll = values[7].isValid() ? values[7].getValue() : null;
        yawRate = values[8].isValid() ? values[8].getValue() : null;
        ax = values[9].isValid() ? values[9].getValue() : null;
        ay = values[10].isValid() ? values[10].getValue() : null;
        az = values[11].isValid() ? values[11].getValue() : null;
        
        gnssTimeType = GnssTimeType.UTC;
        arrivalTime = sample.getOffset();
        
        if (values[12].isValid() && values[13].isValid() && values[14].isValid() &&
            values[15].isValid() && values[16].isValid() && values[17].isValid() && values[18].isValid()) {
            gnssTime = genDateTime((int)values[12].getValue(),
                                   (int)values[13].getValue(),
                                   (int)values[14].getValue(),
                                   (int)values[15].getValue(),
                                   (int)values[16].getValue(),
                                   (int)values[17].getValue(),
                                   (int)values[18].getValue());
        } else {
            gnssTime = null;
        }
        
        return true;
    }
    
    /**
     * 从通用样本V4版本转换为GNSS/IMU数据
     */
    private boolean fromGnssImuSampleV4(GeneralSample sample) {
        GeneralElement[] values = sample.getValues();
        if (values.length != 18) return false;
        
        longitude = values[0].isValid() ? values[0].getValue() : null;
        latitude = values[1].isValid() ? values[1].getValue() : null;
        antennaAltitude = values[2].isValid() ? values[2].getValue() : null;
        speed = values[3].isValid() ? values[3].getValue() : null;
        orientation = values[4].isValid() ? values[4].getValue() : null;
        pitch = values[5].isValid() ? values[5].getValue() : null;
        roll = values[6].isValid() ? values[6].getValue() : null;
        yawRate = values[7].isValid() ? values[7].getValue() : null;
        ax = values[8].isValid() ? values[8].getValue() : null;
        ay = values[9].isValid() ? values[9].getValue() : null;
        az = values[10].isValid() ? values[10].getValue() : null;
        
        locationMode = (longitude != null && latitude != null) ? GnssImuLocationMode.NORMAL : GnssImuLocationMode.NO_LOCATION;
        gnssTimeType = GnssTimeType.UTC;
        arrivalTime = sample.getOffset();
        
        if (values[11].isValid() && values[12].isValid() && values[13].isValid() &&
            values[14].isValid() && values[15].isValid() && values[16].isValid() && values[17].isValid()) {
            gnssTime = genDateTime((int)values[11].getValue(),
                                   (int)values[12].getValue(),
                                   (int)values[13].getValue(),
                                   (int)values[14].getValue(),
                                   (int)values[15].getValue(),
                                   (int)values[16].getValue(),
                                   (int)values[17].getValue());
        } else {
            gnssTime = null;
        }
        
        return true;
    }
    
    /**
     * 是否支持插值
     */
    @Override
    public boolean supportInterpolation() {
        return true;
    }
    
    /**
     * 插值生成中间样本
     */
    @Override
    public Sample interpolate(SearchResult result) {
        GnssImuSample s1 = (GnssImuSample) result.getS1();
        GnssImuSample s2 = (GnssImuSample) result.getS2();
        double w1 = result.getW1();
        double w2 = result.getW2();
        
        GnssImuSample output = new GnssImuSample();
        
        // 位置信息插值
        if (s1.getLocationMode() == s2.getLocationMode()) {
            output.setLocationMode(s1.getLocationMode());
            output.setLongitude(interpolateValue(s1.getLongitude(), s2.getLongitude(), w1, w2));
            output.setLatitude(interpolateValue(s1.getLatitude(), s2.getLatitude(), w1, w2));
            output.setAltitude(interpolateValue(s1.getAltitude(), s2.getAltitude(), w1, w2));
            output.setAntennaLongitude(interpolateValue(s1.getAntennaLongitude(), s2.getAntennaLongitude(), w1, w2));
            output.setAntennaLatitude(interpolateValue(s1.getAntennaLatitude(), s2.getAntennaLatitude(), w1, w2));
            output.setAntennaAltitude(interpolateValue(s1.getAntennaAltitude(), s2.getAntennaAltitude(), w1, w2));
        } else {
            output.setLocationMode(w1 > w2 ? s1.getLocationMode() : s2.getLocationMode());
            output.setLongitude(w1 > w2 ? s1.getLongitude() : s2.getLongitude());
            output.setLatitude(w1 > w2 ? s1.getLatitude() : s2.getLatitude());
            output.setAltitude(w1 > w2 ? s1.getAltitude() : s2.getAltitude());
            output.setAntennaLongitude(w1 > w2 ? s1.getAntennaLongitude() : s2.getAntennaLongitude());
            output.setAntennaLatitude(w1 > w2 ? s1.getAntennaLatitude() : s2.getAntennaLatitude());
            output.setAntennaAltitude(w1 > w2 ? s1.getAntennaAltitude() : s2.getAntennaAltitude());
        }
        
        output.setSatelliteCount(w1 > w2 ? s1.getSatelliteCount() : s2.getSatelliteCount());
        
        // 运动信息插值
        output.setSpeed(interpolateValue(s1.getSpeed(), s2.getSpeed(), w1, w2));
        output.setOrientation(interpolateAngle(s1.getOrientation(), s2.getOrientation(), w1, w2));
        output.setPitch(interpolateValue(s1.getPitch(), s2.getPitch(), w1, w2));
        output.setRoll(interpolateValue(s1.getRoll(), s2.getRoll(), w1, w2));
        output.setYawRate(interpolateValue(s1.getYawRate(), s2.getYawRate(), w1, w2));
        output.setAx(interpolateValue(s1.getAx(), s2.getAx(), w1, w2));
        output.setAy(interpolateValue(s1.getAy(), s2.getAy(), w1, w2));
        output.setAz(interpolateValue(s1.getAz(), s2.getAz(), w1, w2));
        output.setSlipAngle(interpolateValue(s1.getSlipAngle(), s2.getSlipAngle(), w1, w2));
        output.setJerkX(interpolateValue(s1.getJerkX(), s2.getJerkX(), w1, w2));
        output.setJerkY(interpolateValue(s1.getJerkY(), s2.getJerkY(), w1, w2));
        
        // 时间信息插值
        output.setGnssTimeType(s1.getGnssTimeType());
        output.setArrivalTime(interpolateValue(s1.getArrivalTime(), s2.getArrivalTime(), w1, w2));
        
        if (s1.getGnssTime() != null && s2.getGnssTime() != null) {
            long diffMs = s2.getGnssTime().getTime() - s1.getGnssTime().getTime();
            Date interpolatedTime = new Date(s1.getGnssTime().getTime() + Math.round(diffMs * w2 / (w1 + w2)));
            output.setGnssTime(interpolatedTime);
        }
        
        // V7特有属性插值
        output.setPitchRate(interpolateValue(s1.getPitchRate(), s2.getPitchRate(), w1, w2));
        output.setRollRate(interpolateValue(s1.getRollRate(), s2.getRollRate(), w1, w2));
        output.setHorizontalError(interpolateValue(s1.getHorizontalError(), s2.getHorizontalError(), w1, w2));
        output.setVerticalError(interpolateValue(s1.getVerticalError(), s2.getVerticalError(), w1, w2));
        output.setCurvature(interpolateValue(s1.getCurvature(), s2.getCurvature(), w1, w2));
        
        return output;
    }
    
    /**
     * 对角度进行插值，使用正弦和余弦计算
     */
    private Double interpolateAngle(Double a1, Double a2, double w1, double w2) {
        if (a1 == null || a2 == null) return null;
        
        double deg2rad = Math.PI / 180;
        double x1 = Math.cos(a1 * deg2rad);
        double y1 = Math.sin(a1 * deg2rad);
        double x2 = Math.cos(a2 * deg2rad);
        double y2 = Math.sin(a2 * deg2rad);
        
        double xo = x1 * w1 + x2 * w2;
        double yo = y1 * w1 + y2 * w2;
        if (xo == 0 && yo == 0) return null;
        
        return Math.atan2(yo, xo) / deg2rad;
    }
    
    /**
     * 对数值进行插值，处理null情况
     */
    private Double interpolateValue(Double v1, Double v2, double w1, double w2) {
        if (v1 == null && v2 == null) return null;
        if (v1 == null) return v2;
        if (v2 == null) return v1;
        return v1 * w1 + v2 * w2;
    }
    
    /**
     * 生成Date对象，处理无效日期
     */
    private Date genDateTime(int year, int month, int day, int hour, int minute, int second, int ms) {
        if (year < 2000 || year > 3000) return null;
        if (month < 1 || month > 12) return null;
        if (day < 1 || day > 31) return null;
        if (hour < 0 || hour > 23) return null;
        if (minute < 0 || minute > 59) return null;
        if (second < 0 || second > 59) return null;
        if (ms < 0 || ms > 999) return null;
        
        try {
            Calendar calendar = new GregorianCalendar(year, month - 1, day, hour, minute, second);
            calendar.set(Calendar.MILLISECOND, ms);
            return calendar.getTime();
        } catch (Exception e) {
            return null;
        }
    }
    
    // 协议相关常量
    public static final String TARGET_PROTOCOL = "gnssimu-sample-v7";
    public static final String[] PROTOCOLS = {
        "gnssimu-sample-v4", "gnssimu-sample-v5", "gnssimu-sample-v6", "gnssimu-sample-v7"
    };
} 